#include "yyh.h"
#include "Arduino.h"
#include <IRremote.h>
#include <stdio.h>
#include <stdlib.h>
#include "SevSeg.h"
Servo myservo;
LiquidCrystal_I2C lcd(0x27,16,2);
SevSeg sevseg;
int YYH::getTime(){
    times_now = millis()-times_before;
    times_before = millis();
    return times_now;
}
void YYH::init(){
    Serial.begin(9600);
    lcd.init();
    lcd.backlight();
    lcd.clear();
    lcd.setCursor(0,0);
}
void YYH::print(const char c[],uint8_t a,uint8_t b){
    if(a!=0||b!=0){
      lcd.setCursor(a,b);
    }
    Serial.println(c);
    lcd.print(c); 
}
void YYH::print(int c,uint8_t a,uint8_t b){
    if(a!=0||b!=0){
      lcd.setCursor(a,b);
    }
    char str[25];
    Serial.println(c);
    itoa(c,str,10);
    lcd.print(str);
}
void YYH::print(char c,uint8_t a,uint8_t b){
    if(a!=0||b!=0){
      lcd.setCursor(a,b);
    }
    Serial.println(c);
    lcd.print(c); 
}
void YYH::clear(){
    lcd.clear(); 
}
void YYH::servo(uint8_t a,uint8_t b){
    if(pinmode[b]==0){
      myservo.attach(b);
      pinmode[b]=1;
    }
    myservo.write(a);
}
int YYH::csb(uint8_t a,uint8_t b){
    if(pinmode[a]==0){
      pinMode(a, INPUT);
      pinMode(b, OUTPUT);
      pinmode[a]=1;
    }
    digitalWrite(b, LOW);
    delayMicroseconds(2);
    digitalWrite(b, HIGH); // Pulse for 10μs to trigger ultrasonic detection
    delayMicroseconds(10);
    digitalWrite(b, LOW);  
    int distance = pulseIn(a, HIGH);  // Read receiver pulse time
    distance= distance/58;   // Transform pulse time to distance
    delay(50);
    return distance;
}
char YYH::getval(){
    char keys[4][4] = {
    {'1','2','3','A'},
    {'4','5','6','B'},
    {'7','8','9','C'},
    {'*','0','#','D'}
    };
    byte rowPins[4] = {5,4,3,2};
    byte colPins[4] = {6,7,8,9};
    Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, 4, 4);
    while(1){
      char key = keypad.getKey();
      if (key != NO_KEY) {
          delay(200);
          return key;
        }
        delay(200);
      }
    
}
void YYH::write(int pin,int val){
    if(pinmode[pin]==0){
        pinMode(pin,OUTPUT);
        pinmode[pin]=1;
    }
    digitalWrite(pin,val);
}
int YYH::recv(int pin){
     IRrecv irrecv(pin);
     decode_results results;
    if(pinmode[pin]==0){
      irrecv.enableIRIn(); 
      pinmode[pin]=1;
      Serial.println("设置成功");
    }
      if(irrecv.decode(&results)) {
        Serial.println(results.value, HEX);
        irrecv.resume();
        delay(200);
        switch(results.value){
          case 0xFFA25D:
          return 1;
          case 0xFF629D:
          return 2;
          case 0xFFE21D:
          return 3;
          case 0xFF22DD:
          return 4;
          case 0xFF02FD:
          return 5;
          case 0xFFC23D:
          return 6;
          case 0xFFE01F:
          return 7;
          case 0xFFA857:
          return 8;
          case 0xFF906F:
          return 9;
          default:
          return results.value;
      }
  }else{
      irrecv.resume();
      delay(200);
      return 0;
  }

}
int YYH::read(int pin){
  if(pinmode[pin]==0){
      pinMode(pin,INPUT);
      pinmode[pin]=2;
  }
  return digitalRead(pin);
}
int YYH::aread(int pin){
  return analogRead(pin);
}
int YYH::tep(int pin){
  int val = analogRead(pin);
  return (125*val)>>8;
}
void YYH::pwm(int pin,int val){
  if(pinmode[pin]==0){
      pinMode(pin,OUTPUT);
      pinmode[pin]=1;
  }
  analogWrite(pin,val);
}
void YYH::flash(int pin,int num,int val){
  if(pinmode[pin]==0){
      pinMode(pin,OUTPUT);
      pinmode[pin]=1;
  }
  for(int i=0;i<num;i++){
     digitalWrite(pin,1);
     delay(val);
     digitalWrite(pin,0);
     delay(val);
  }
}
void YYH::smg(int val){

  if(pinmode[2]==0){
      pinmode[2]=1;
      byte numDigits = 4;     // 数码管位数
      byte digitPins[] = {2, 3, 4, 5};  //数码管公共极连接Arduino的引脚
      byte segmentPins[] = {6, 7, 8, 9, 10, 11, 12, 13}; //数码管a,b,c,d,e,f,g,dp对应引脚
      bool resistorsOnSegments = false; // 'false' 表示电阻器在数字引脚上
      byte hardwareConfig = COMMON_ANODE; // 使用的是共阳极数码管
      bool updateWithDelays = false; // Default 'false' is Recommended
      bool leadingZeros = false; // 如果你想保持前导零，就用true
      bool disableDecPoint = false; //如果你的小数点不存在或没有关联，请使用true
      sevseg.begin(hardwareConfig, numDigits, digitPins, segmentPins, resistorsOnSegments,
      updateWithDelays, leadingZeros, disableDecPoint);//添加参数
      sevseg.setBrightness(6);
  }
   sevseg.setNumber(val); //设置要显示的数据，不显示小数点
    for(int j=0;j<32000;j++){
      sevseg.refreshDisplay();  // 必须重新运行刷新数码管显示
      j++;
    }
}
void YYH::smgclear(){
  sevseg.blank();
  for(int j=0;j<32000;j++){
    sevseg.refreshDisplay();  // 必须重新运行刷新数码管显示
    j++;}
}
double YYH::pid(double kp,double ki,double kd,double actual,double target,double time_delta){
    //计算期望值和实际值之间的差值（误差）
    double error = target - actual;
    //计算和更新积分
    integral += (error * time_delta);
    //计算导数
    double derivative = (error - previous_error) / time_delta;
    //根据算法计算输出值
    double output = (
        (kp * error) + (ki * integral) + (kd * derivative)
    );
    //将前一个错误更新为此迭代计算的错误值
    previous_error = error;
    //返回计算结果
    return output;
}
